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I.  INTRODUCTION 


A.  GENERAL 

Stated  most  simply,  the  problem  of  navigation  can  be  summarized  by  the  following 
three  questions;  "where  am  I?",  "where  am  I  going?",  and  "how  should  I  get  there?". 
The  Hist  question  is  one  of  localization;  "how  can  I  work  out  where  I  am  in  a  given 
environment,  based  on  what  I  can  see  and  what  I  have  previously  been  told?".  The 
second  and  the  third  questions  are  essentially  those  of  specifying  a  goal  and  being  able 
to  plan  a  path  that  results  in  achieving  this  goal.  Investigation  of  the  latter  two  questions 
usually  comes  under  the  domain  of  path  planning  and  obstacle  avoidance  [Ref.  1].  In  this 
thesis,  we  are  principally  concerned  with  the  first  question,  localization,  since  a  robust 
and  reliable  solution  to  this  problem  is  essential  to  answering  the  remaining  two 
questions. 

An  Autonomous  Underwater  Vehicle  is  a  type  of  Unmanned  Underwater  Vehicle 
(UUV),  that  is  not  limited  by  the  need  for  local  human  control.  The  freedom  from 
requiring  an  external  control  interface  theoretically  allows  this  type  of  vehicle  to  perform 
a  great  range  of  missions  [Ref.  2].  To  widen  the  range  of  application  of  an  AUV,  it  is 
necessary  to  develop  systems  with  high  levels  of  autonomy  and  to  operate  in  unstructured 
environments  with  little  a  priori  information.  To  achieve  this  degree  of  indq)endence, 
the  AUV  must  have  an  understanding  of  its  surroundings,  by  acquiring  and  manipulating 
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a  rich  model  of  its  environment  of  qjeration  [Ref.  3].  While  autonomy  has  clear 
advantages,  it  requires  a  sophisticated  level  of  on  board  processing  ability. 

B.  AIM  OF  THE  STUDY 

This  thesis  is  concerned  with  the  navigation  problem  of  an  AUV  in  the  horizontal 
plane.  For  any  navigation  system  to  be  useful,  the  accuracy  of  the  instruments  has  to  be 
such  that  the  navigator’s  error  is  within  the  required  tolerance  of  the  vehicle  which  relies 
on  the  navigation  system.  However,  accurate  sensors  such  as  high  quality  inertial 
navigation  systems  and  doppler  sonars  tend  to  be  large  and  expensive,  and  unsuitable  to 
a  small  vehicle.  It  is  the  aim  of  this  thesis  to  study  the  feasibility  of  combining 
measurements  from  the  sonar  device  on  board  to  generate  relatively  good  position 
estimates  over  a  short  time  interval. 

C.  METHOD  OF  APPROACH 

This  thesis  is  concerned  with  the  short-range  navigation  problem  for  the  NFS 
testbed  AUV.  Being  limited  in  complexity  and  cost,  the  navigator  has  to  rely  on  the 
available  hardware.  Also,  because  the  system  needs  only  to  cerate  over  short  ranges, 
the  effects  of  the  earth’s  orbit  and  rotation  can  be  neglected.  This  approximation  also  has 
the  effect  of  simplifying  the  model  to  be  built. 

The  approach  taken  in  this  thesis  to  solve  the  navigation  problem  in  the  horizontal 
plane  is  to  use  a  nonlinear  dynamic  model  of  the  vehicle  to  filter  the  sonar  range  returns 
for  estimation  of  the  vehicle’s  position.  The  measurements  obtained  from  sonar  and 
nonlinear  model  are  run  through  the  Kalman  filter  to  calculate  the  Kalman  gain.  Then, 
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the  Kalman  gain  is  used  to  make  the  next  position  estimation  of  nonlinear  model.  This 
i^)(»oach  is  diagramed  in  Figure  1.1.  The  filter  to  be  used  is  an  Extended  Kalman  filter. 


Figure  1 . 1  Kalman  Filtering  Concept 


The  Kalman  filter  theory,  which  ai^)eared  in  the  early  I960’s,  was  recognized  as 
an  ideal  solution  to  the  navigation  data  processing  problems  for  navigation.  These  data 
processing  problems  can  be  adapted  nicely  into  the  necessary  Kalman  filter  assumptions, 
which  means  that  the  estimates  of  the  desired  navigation  outputs  are,  in  fact,  very  nearly 
optimum.  This  gives  the  engineer  confidence  that  the  system  is  the  best  that  can 
reasonably  be  expected.  Also,  the  recursive  form  of  the  filter  is  convenient.  A  new 
optimum  estimate  can  be  made  very  shortly  after  each  new  measurement  is  obtained.  Nor 
is  it  necessary  to  store  a  great  amount  of  data  or  inveit  a  large  matrix,  as  might  be 
necessary  in  more  conventional,  least-squares  fitting  techniques  [Ref.  4]. 

Navigation  systems  are  among  the  most  popular  areas  for  the  application  of  Kalman 
filtering.  Most  of  the  major  navigation  system  manufacturers  have  developed  or  proposed 
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systems  based  on  Kalman  f.>ering  techniques,  and  they  are  being  used  in  several  vehicles 
in  operational  use.  Ivalman  filtering  has  now  become  an  integral  part  of  almost  any 
navigatitm  system.  The  reasons  for  the  popularity  of  Kalman  filtering  in  navigation 
are  not  hard  to  find.  There  are  at  least  three  major  complementary  factors  that 
have  come  u^ether  at  the  proper  time.  These  factors  are  an  increased  need  for  self 
contained  navigation  systems,  the  mathematical  tools  to  design,  and  the  necessary 
equipment  to  implement  [Ref.  4]. 

This  thesis  is  presented  in  four  parts.  Chapter  n  discusses  background  and  the 
theory  behind  the  navigator  design  and  presents  some  results  for  a  model  designed  to 
operate  in  a  pool  without  any  obstacles  in  the  (^ration  environment.  Chapter  in 
discusses  the  details  of  potential  function  technique  used  in  simulations  and  the 
autor^ressive  extended  (ARX)  model  based  parameter  estimation.  Finally,  Chapter  IV 
summarizes  the  results  of  this  research  and  contains  conclusions  and  recommendations 
for  application  and  further  study. 


n.  A  GEOMETRICAL  MODEL  OF  ENVIRONMENT 


A.  GENERAL 

In  this  chapter,  we  set  up  the  nonlinear  model  for  the  dynamics  of  an  AUV  and  the 
model  for  sonar  measurements.Then,  we  define  the  experiment  in  a  pool  without 
obstacles  and  implement  Kalman  filtering  techniques  for  estimating  the  position  of  the 
vehicle.  In  the  estimation  process,  we  use  the  sonar  range  measurements  only. 

B.  DISCRETE  KALMAN  FILTER  EQUATIONS 

The  Kalman  filter  estimates  the  state  of  a  system  given  a  set  of  known  inputs  to  the 
system  and  a  set  of  measurements  [Ref.  5].  The  system  is  assumed  to  be  driven  by  both 
a  known  input  and  an  unknown  raiKlom  input: 

jc()t+l)=4>jc(it)+Aj«(ik)+A2w(*)  ,  (  2.1 ) 

where  u(k)  is  the  known  input,  w(k)  is  the  random  input  called  the  plant  driving  noise, 
x(k)  is  the  state  vector,  #  is  state  transition  matrix.  A)  is  known  input  matrix,  and  Aj  is 
random  input  matrix.  We  assume  that  w(k)  is  white  noise. 

The  measurements  of  the  system  are  related  to  the  state  by 

y{k)=Hx{k)Mk)  ,  (  2.2  ) 

where  y(k)  is  the  measurement,  H  is  the  measurement  matrix,  and  v(k)  is  the  random 
measurement  noise.  We  assume  that  v(k)  is  white  noise. 
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The  solution  of  the  estimation  problem  can  be  shown  to  have  the  following  form: 


je(*:+l \k*l)=Ji(k*l\kyGik^l)\y{k*l)-Sik^l)]  ,  (  2.3  ) 

where  G(k+1)  is  the  Kalman  filter  gain  at  the  given  specific  time. 

The  Kalman  filter  gain  G(k+1)  is  found  by  i^)plying  the  orthogonality  principle 
which  leads  to  the  recursive  equations: 

P(k*l 

G(ife+l)=P(it+l \k)H'^*V\'^  ^  ^ 

P(k*l\k*l)=[I-Gik*l)H]Pik*l\k)  , 

where  P  is  the  covariance  matrix  of  the  estimation  error.  The  following  is  the  summary 
of  the  Kalman  filter  equations  in  the  order  in  which  they  are  implemented  throughout  this 
thesis: 

Th?  EgwrtioBs; 

Pik+l  \k)=9Pik\k)<I^^+^2^^/ 

G(k*l)=P{k+l\k)H^HPik+l \k)H^*V]'^  ^  ^ 

Pik*  1 1*+ 1) =II-G(k*  l)H]Pik*  1  |/t) 

The  Filter  Equations: 

|*:)=®i(it|il:)+A,ii(A:) 

S(k+l\k)=mik+l\k)  (2.6) 

i(it+l  |it+l)=je(it+l  (it)+G(ife+l)[y(ife+l)-^ifc+l  |A:)] 
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This  is  known  as  the  predictor-corrector  form  of  the  Kalman  filter  in  which  the 
iMxt  state  is  predicted  using  the  state  space  model  and  the  measurement  is  used  to  correct 
the  prediction.  [Ref.  5] 

The  W  matrix  is  the  covariance  of  tl^  system’s  noise,  while  the  Aj  matrix 
describes  the  way  this  noise  enters  the  system.  If  W  is  diagonal,  then  the  disturbances 
are  assumed  to  be  independent,  otherwise  they  are  correlated,  as  described  by  the  off- 
diagonal  terms  in  W.  [Ref.  5] 

The  V  matrix  describes  measurement  noise  from  the  sensors.  The  Kalman  filter 
considers  the  measurement  noise  to  enter  all  the  individual  measurements.  As  with  the 
measurement  noise  covariance  matrix,  if  V  is  large,  then  the  measurements  are  e  ;pected 
to  deviate  more  from  the  states  being  measured,  and  the  Kalman  filter  will  rely  more  on 
the  predicted  state  than  on  the  measurements.  [Ref.  5] 

The  initial  P  matrix  affects  the  dependence  the  Kalman  filter  has  on  the  initial 
conditions.  Large  values  in  initial  P  mean  that  the  filter  will  not  depend  on  the  initial 
conditions,  but  will  instead  give  more  weight  to  the  measurements.  This  allows  the 
estimated  state  to  change  rapidly  as  the  filter  goes  through  its  transient  stage.  In  steady 
state,  however  P  has  no  effect  on  the  Kalman  filter  because  it  ^roaches  a  constant 
value  that  is  dependent  only  on  the  system  and  the  noise  covariance  matrices.  [Ref.  5] 
Because  the  gain  equations  do  not  depend  directly  on  time  or  on  the  state  trajectory 
in  a  LTl  system,  the  gain  can  be  calculated  a  priori  and  recalled  from  memory  as 
needed.  There  is  no  need  for  real-time  gain  computation.  Moreover,  in  the  time  invariant 
case  the  gain  matrix  approaches  a  steady-state  value,  which  is  determined  by  the  system 
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equations  and  the  W  and  V  matrices  through  the  associated  Kalman  filter  equations.  In 
many  cases,  using  the  steady-state  gain  matrix  instead  of  the  time-varying  gain  matrix 
gives  satisfactory  results  in  a  reduced-complexity  algorithm  which  does  not  take  into 
account  prior  knowledge  of  the  initial  conditions.  [Ref.  S] 

C.  LINEARIZATION 

Thus  far,  the  discussion  has  been  limited  to  linear  systems;  however,  many  systems 
in  which  the  control  engineer  is  interested  are  nonlinear.  Nonlinear  system  models  are 
more  general  than  linear  models  and  can  contain  a  wide  variety  of  nonlinear 
characteristics  for  which  limited  analytic  tools  exist.  In  general,  nonlinear  systems  do  not 
exhibit  the  prc^rties  of  homogeneity  or  supeiposition,  and  many  include  transcendental, 
trigonometric  or  other  nonlinear  functions  [Ref.  6].  Such  is  the  case  with  the  model 
chosen  for  this  thesis. 

A  method  for  woiking  with  nonlinear  systems  is  to  linearize  them  using  a  truncated 
Taylor  series  approximation.  The  Taylor  series  ai^roximates  a  function  around  a  given 
point  as  an  infmite  sum  of  weighted,  analytically  determined,  partial  derivatives.  A 
general  Taylor  series  around  the  point  x„  is  given  by: 

ii-i  n!<£t” 

A  truncated  Taylor  series  only  uses  a  few  of  the  terms  of  the  sum.  In  order  to 
realize  a  linear  function  from  the  Taylor  series  expansion  of  a  nonlinear  one,  the  series 
must  be  truncated  at  the  first  term.  This  model  is  general  and  can  be  applied  to  any 
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nonlinear  system,  and  it  will  be  valid  in  a  region  surrounding  the  linearization  point  [Ref. 
7]. 

For  the  case  of  a  continuous-time  dynamic  system  represented  in  state-space  form, 
which,  in  general,  is  not  a  function  of  a  single  variable,  but  is  rather  a  function  of  time, 
the  input  vector,  and  the  past  state  vector,  the  Taylor  series  is  defmed  over  the  partial 
derivatives  of  each  of  the  indqx^ndent  variables.  In  the  case  of  a  time-invariant  system, 
the  series  is  expanded  about  an  operating  point  described  by  the  state,  Xo,  and  a 
corresponding  input,  Ug.  A  nonlinear  state  space  equation  can  then  be  aj^roximated  as 


obc 


diM 


(2.8) 


where  f  is  the  nonlinear  system  function.  This  notation  implies  that  the  partial  derivatives 
are  constant  terms,  calculated  analytically  and  evaluated  at  x„,  and  u^  [Ref.  8]. 

Just  as  a  nonlinear  system  equation  can  be  expanded  using  a  Taylor  series,  and 
linearized,  a  nonlinear  measurement  equation  can  also  be  e^qTanded  and  linearized.  The 
formulation  for  this  expansion  is  similar  to  the  equation  2.8. 


D.  EXTENDED  KALMAN  FILTER 

The  Kalman  filter  is  derived  for  linear  systems  with  linear  measurement  equations; 
however,  using  the  linearization  techniques  described  in  the  last  section,  this  filter  can 
find  application  to  general,  nonlinear  systems.  This  is  known  as  the  Extended  Kalman 
rilter.  The  Extended  Kalman  filter  is  suboptimal  and  may  suffer  convergence  and  stability 
problems,  but  it  has  been  shown  to  be  useful  in  variety  of  sqiplications.  [Ref.  5] 
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The  form  of  the  Extended  Kalman  filter  equations  are  essentially  similar  to  those 
of  the  Linear  Kalman  filter.  The  nonlinear  model  is  used  to  predict  the  state  and  the 
nonlinear  measurement  is  used  to  correct  the  prediction.  The  most  significant  difference 
between  the  Extended  Kalman  filter  and  the  Linear  Kalman  filter  is  the  gain  equations. 
Rather  than  using  #  (the  state  transition  matrix),  A  (the  input  matrix),  and  H  (the 
measurement  matrix)  matrices  to  calculate  the  gain,  the  Extended  Kalman  filter  uses  the 
linearized  model  of  the  nonlinear  system.  It  uses  the  partial  derivatives  of  the  nonlinear 
state  equations  and  the  nonlinear  measurement  equations.  These  partials  are  evaluated  at 
each  estimated  state.  As  such,  the  gain,  G,  caimot  be  computed  in  advance,  because  it 
is  d^ndent  on  both  the  state  trajectory  and  the  input  history. 

Calculating  partial  derivatives  of  the  nonlinear  system  and  measurement  equations 
for  each  measurement  as  well  as  calculating  the  Kalman  filter  gain  matrix  is  a 
computational  burden.  In  order  to  overcome  this  problem,  it  may  be  possible  to  use  gain 
matrices  calculated  in  advance  and  by  choosing  discrete  points  in  the  system’s  state  space 
about  which  to  linearize  the  nonlinear  sy^^.  Practically,  in  choosing  the  points,  it  is 
convenient  that  only  some  of  the  system  states  are  varied  while  others  are  kq)t  constant. 
Using  the  chosen  points,  several  linear  approximations  to  the  nonlir^ar  system  are 
calculated  and  then  used  to  calculate  the  steady-state  Kalman  filter  gain  matrix  associated 
with  those  particular  points.  Once  the  gains  are  calculated,  the  Extended  Kalman  filter 
is  implemented  by  determining  which  of  the  chosen  linearization  points  are  closest  to  the 
current  estimated  state  and  the  corre^nding  gain  matrix  is  used  in  the  Extended  Kalman 
filter  equation  as  given  as  the  last  of  Equations  2.6.  [Ref.  S] 
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B  In  using  the  Extended  Kalman  filter,  nonlinearities  are  modeled  as  plant  noise.  This 
means  that  the  Aj  matrix  is  usually  the  identity  matrix.  Furthermore,  because  the 
nonlinear  effects  are  not  included  in  the  gain  equations,  the  Extended  Kalman  filter  can 
be  very  sensitive  to  the  W  and  V  matrices.  Choosing  improper  values  can  make  an 
Extended  Kalman  filter  unstable.  The  values  chosen  for  these  matrices  should  not 
necessarily  correspond  to  the  actual  noise  expected  in  the  system  or  in  the  measurements, 
but  rather  they  must  be  made  large  enough  to  provide  a  robust  prediction  in  spite  of  the 
nonlinear  effects. 


E.  MODELLING  FOR  PLANAR  MOTION  OF  THE  AUV 

The  basic  component  of  the  estimation  process  is  a  state  vector  whose  value  at  any 
time  t  is  given  by: 


where(Xv,yv)defmesthepositionofthevehiclewiUispeedv,  heading  ff,  yaw  rate  6  .The 
subscript  "v"  is  used  to  distinguish  the  position  of  vehicle  from  the  state  vector  x  and 
measurement  y. 
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liqxit  to  the  system  are  propeller  ^)eed,  u,,  and  vertical  rudder  command,  Uj. 

( 2.10 ) 

The  differential  equations  forming  to  our  model  are  as  follows: 

V  cosd 

V  sinO 

-«  v+p  ^  (  2.11 ) 

6 

-y  6+0  ttj 

where  a,  /3,  y,  and  a  are  constants  dq)ending  on  the  dynamics  of  the  vehicle. 

As  we  can  see,  equations  2. 1 1  are  nonlinear  and  they  have  to  be  linearized  for 
further  analysis.  Now  let  us  apply  truncated  Taylor  series  expansion  to  the  first  two 
equations.  In  the  following  equations  the  subscript  "o”  indicates  the  current  value  of  the 
particular  state  around  which  we  linearize  the  model, 


i^=v^oose,+ 


(v-v^ 


3(iv(v,,e^) 


(e-ej 


(2.12) 


yv=v^sme^+ — -(Q-Oo) 


(2.13) 


where  Vo  and  are  the  current  speed  and  heading  of  AUV  respectively.  Now  if  we 
follow  the  algebra  involved  in  equations  2.12  and  2. 13  we  can  find  the  following  results: 
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(  2.14  ) 


=vco80.  -dv^sine, +0*v,sine, 
>„=vsm0„+ev„cos0--6-v„cos0„  ( 2.15 ) 

•'V  o  o  o  o  o  o 

The  last  two  terms  in  the  equations  2. 14  and  2.  IS  are  almost  identical;  they  differ  in  one 
sign.  Now  the  following  approximations  can  be  made: 

i^=vcos6^  ( 2.16 ) 

y^=vsin0^  ( 2.17 ) 

In  the  model  introduced  above,  sonar  range  is  the  only  measurement  that  depends 
on  the  current  position  and  orientation  of  the  AUV  with  respect  to  the  environment.  The 
parameter  is  sonar  heading  which  is  measured  with  re^)ect  to  the  x-axis  of  the  vehicle 
and  defines  the  angle  b^een  x-axis  and  the  sonar  beam  at  the  measurement  time. 

( 2.18 ) 

In  this  study,  we  address  the  problem  of  navigating  the  vehicle  in  a  geometric 
environment,  like  a  pool  or  tank  with  known  dimensions  L,  by  L,.  Figure  2. 1  shows  the 
setup  for  range  measurements  .  The  n  parameters  define  the  particular  angle  computed 
between  x-axis  and  comers  of  the  pool,  taking  the  current  position  of  the  vehicle  as  the 
origin.  Range  r,  sonar  heading  y,  and  vehicle  position  (x,,  y,)  are  related  by  the 
geometry  of  the  environment  as 
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(  2.19  ) 


n3^Y<Ti2 


(  2.20  ) 


n4^Y<^3 


( 2.21 ) 


( 2.22  ) 


Figure  2.1  Determination  of  sonar  range  in  the  pool 


If  we  call  H  the  measurement  matrix  defined  as 


H=[^  .^000 

dx  dy 


(2.23  ) 


then  we  can  compute  it  from  the  equations  2.19  through  2.22  as 
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0  0  0  0 j  ;  (2.24) 

^=1^0  0  0  0 j  ;  rij^y<ti^  ;  r|^^y<r|^  (2.25) 

In  the  light  of  the  preceding  discussion,  the  state  i^ce  rq>resentation  of  the 
particular  model  can  be  defmed  as: 


;c= 


0  0  cos6^  0  0 

0  0 

0  0  sinO^  0  0 

0  0 

0  0  -a  0  0 

x+ 

b  0 

0  0  0  0  1 

0  0 

0  0  0  0  -c 

P  d 

B<+W 


y=Hx+v 


( 2.26 ) 


( 2.27 ) 


F.  SIMULATION  RESULTS 

Figures  2.2  through  2.10  show  the  simulated  vehicle  trajectory,  as  determined  by 
the  nonlinear  model.  In  all  the  cases,  the  vehicle  was  given  a  constant  propeller  speed 
of  140  RPM.  It  starts  from  an  estimated  position  when  it  is  at  rest  initially.  After  the 
convergence  of  the  Kalman  filter  to  the  simulated  trajectory,  a  rudder  command  is  given 
for  a  turn  to  be  executed.  In  smooth  turns,  like  shown  in  Figures  2.2  through  2. 10,  the 
model  is  able  to  track  the  actual  simulated  trajectory. 
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The  lower  left  hand  comer  of  the  pool  is  accq)ted  as  the  position  reference  point 
for  all  the  runs.  There  are  no  obstacles  present  in  the  pool,  which  yields  to  clear  sonar 
returns  from  the  borders  of  the  pool. 


Figure  2.2  A  sample  run  with  initial  heading  0° 
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Figure  2.10  A  sample  ran  with  a  complete  steady  turn 


in.  POTENTIAL  FUNCTION  APPROACH  FOR  ENVIRONMENT 

A.  GENERAL 

In  this  chapter,  we  will  approach  the  problem  of  navigation  with  the  same  model 
used  in  Chapter  n  by  using  a  potential  function,  which  is  intended  to  define  the 
environment. 

B.  MODELLING  ASSUMPTIONS 

The  basic  component  of  the  estimation  process  is  based  on  the  state  vector  which 
is  defined  in  equation  2.  IS.  This  leads  to  a  dynamic  model  of  the  form 

with  z„  6  R®  the  state  of  the  vehicle,  u  €  R’  the  vector  of  external  commands  (RPM  and 
RUDDER  ANGLE)  and  w,  disturbances,  modelling  errors  between  model  and  the  actual 
physical  dynamics.  [Ref.  9] 

The  measurement  vector  consists  of  dynamic  observations  of  sonar  range  p  at  an 
angle  a  with  respect  to  the  longitudial  axis  of  the  vehicle  with  v  indicating  measurement 
noise 

y=«(iv.P,a.v)  .  (3.2) 

We  assume  the  noise  sources  w  and  v  to  be  of  zero  mean,  white,  and  Gaussian,  as 
is  standard  in  this  class  of  problems.  The  function  g  is  defined  by  the  map  of  the 
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environment,  and  s^)art  from  measurement  noise,  it  is  zero  when  the  sonar  return 
information  (p,  a)  is  consistent  with  the  vehicle  position  with  respect  to  the  map.  The 
criterion  we  use  to  choose  g  is  critical  for  this  thesis,  and  it  is  based  on  the  use  of  a 
potential  function  properly  defmed.  For  a  vehicle  moving  on  a  plane  and  located  by 
position  (x,y)  and  heading  6,  the  function  g  is  defmed  as 

S(Xty.0iP »«) = pco8(e + a)o>+ psin(0 + a))  0.3) 

The  vector 

[jc^jJ=[jc+pcos(0+a)^+psm(0+a)]  (3.4) 

denotes  the  location  of  the  tip  of  the  sonar  range  vector  in  the  given  reference  frame. 
With  this  definition,  function  V  has  to  satisfy  the  following  necessary  conditions. 

•  V  is  continuous  and  differentiable 

•  V(x<„yo)=0  if  the  point  is  on  a  reflecting  surface  defined  on  the  map 

•  V  is  uniformly  bounded  over 

Considering  a  pool  of  rectangular  shape  with  sides  L,  by  Lj  units,  and  by  taking 
the  lower  left  comer  as  the  origin  of  our  map  and  two  adjacent  sides  as  axis,  we  can 
write  the  potential  function  V  as 

Vix;y)-F^{xix-L^yy(y-L^))  (3.5) 

We  choose  in  equation  3.5  in  order  to  provide  boundedness  for  V.  In  particular,  the 
choice  of  a  "squashing"  function  such  as  the  sigmoid 
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(3.6) 


for  some  positive  X,  will  make  the  potential  V  satisfy  the  conditions  stated  above. 

The  parameter  X  in  the  squashing  function  has  to  be  chosen  as  a  compromise 
between  two  conflicting  necessities: 

•  large  enough  to  provide  a  sufficient  region  of  attraction  to  correct  for  errors  in 
estimates 

•  small  enough  so  that  objects  which  are  in  the  field  of  operation  and  not  on  the  map 
are  outside  the  region  of  attraction 


In  the  sqjplication  shown  below  we  u%  a  time  varying  X  decreasing  with  time  in 
order  to  accommodate  larger  errors  at  the  beginning  of  the  estimation  processes.  The 
estimate  of  the  state  z  is  just  a  standard  Extended  Kalman  filtering  operation,  which  is 
basically  deflned  in  Chapter  n. 


C.  SIMULATION  RESULTS 

The  navigation  algorithm  has  been  tested  on  a  rectangular  pool  with  sides  L,  by  L,. 
Figure  3.1  shows  typical  contours  of  potential  function,  while  Figure  3.2  is  the  three 
dimensional  plot  of  the  potential  function  over  the  pool. 

The  sonar  returns  are  recorded  from  several  scans  spanning  the  whole  360°  circle 
at  intervals  of  0.9°,  thus  resulting  in  400  sonar  returns  per  scan.  The  source  codes  used 
during  the  simulations  are  listed  in  Appendix-B.  The  programs  SCANS.M,  SIMULS.M, 
VP.M,  SIG.M,  and  DSIG.M  are  main  files  listed.  The  numbers  used  in  the  names  of 
codes  indicates  the  version  number  of  a  main  change  in  program  structure. 
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Figure  3.1  The  contours  of  the  potential  function 


potential  FONCTiON  OVER  WATER  TANK 


Figure  3.2  Three  dimensional  potential  function 


The  forward  velocity  estimate,  shown  in  Figure  3.3,  jq^roaches  a  constant  value. 
The  estimation  error  seen  in  the  beginning  of  this  run  is  induced  by  the  random 
disturbances  added  to  the  initial  state.The  yaw  rate  estimate,  shown  in  Figure  3.4,  and 
the  yaw  estimate,  shown  in  Figure  3.S,  exhibits  fairly  good  estimates.  The  initial  errors 
are  due  to  the  initial  estimates  of  the  system  states  and  added  random  distuibances.  The 
yaw  estimate  t^roaches  its  actual  value  after  30  seconds  of  simulation  time, which  shows 
us  the  sensivitivity  of  the  system  to  the  heading  estimates. 


Figure  33  Measured  and  estimated  forward  velocities 
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YAW  RATE  W  DEG 


MEASURED  AND  ESTIMATED  YAW  RATE 


Figure  3.4  Measured  and  estimated  yaw  rate 


MEASURED  and  ESTIMATED  HEADINGS 


0  10  20  30  40  50 

TME  N  SECONDS 


Figure  3.5  Measured  and  estimated  yaw  angle  of  the  AUV 


The  results  of  the  position  estimation  are  shown  in  Figures  3.6  through  3.9,  where 
we  used  the  data  collected  for  seven  successive  360°  scans  for  each  case.  In  Figures  3.6 
and  3.7,  we  assume  the  initial  position  and  orientation  of  the  AUV  to  be  known.  In  these 
figures  the  sequence  of  location  of  the  estimated  reflective  surfaces  is  shown  for  each 
scan,  superimposed  to  the  contour  of  the  potential  function.  The  estimated  trajectory  is 
also  shown,  which  coincides  with  the  actual  induced  motion  of  the  sonar  head.  The 
estimates  of  the  reflection  from  charted  objects  can  easily  be  distinguished  from  the 
charted  regions,  since  the  value  of  the  potential  function  is  close  to  one  for  the  uncharted 
and  zero  for  the  charted.  In  Figures  3.8  and  3.9  we  repeat  the  same  simulation  with  a 
significant  error  in  the  estimate  of  the  initial  location  of  the  sonar  head.  When  the 
original  location  is  unknown,  we  start  with  a  large  enough  value  of  the  parameter  X  and 
make  it  decrease  exponentially.  In  Figure  3.9,  we  notice  that  until  the  convergence  of  the 
Kalman  filter  we  might  g^  some  estimates  that  are  strictly  outside  the  map  defined  by 
the  potential  function.  Then  these  initial  estimates  eventually  converge  to  the  original 
trajectory  of  the  vehicle. 
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Figure  3.9  Estimation  with  unknown  initial  conditions  and  a  steady  turn 
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D.  ESTIMATION  OF  SYSTEM  PARAMETERS 

The  estimation  of  the  system  dynamics  is  the  subject  of  system  identification. 
Identification  has  many  aspects  and  phases,  and  it  is  customary  to  organize  identification 
by  considering  a  certain  number  of  steps  each  time  we  encounter  an  identification 
problem.  It  is  often  natural  to  restrict  the  complexity  of  modeling  to  a  certain  model 
structure.  The  class  of  models  thus  adopted  often  belong  to  some  standard  cat^ory  of 
models  such  as  linear  systems  or  ARMAX  model  associated  with  ceitain  mathematical 
pix^rties.  Another  standard  approach  often  used  is  to  make  assumptions  as  to  the 
physical  nature  of  the  system  or  other  restrictions  that  define  physical  parameterization. 

1.  Model  Structures 

The  ai^roaches  to  modelling  of  linear  systems  and  linear  regression  models 
yields  to  the  following  equation,  in  the  regression  form  as 

Linear  regression  identification  consists  in  reformulating  various  estimation  and 
prediction  problems,  in  the  form  of  equation  3.7,  which  for  suitable  definitions  of  the 
observations  y^,  regressors  and  disturbances  v^  also  a{^lies  directly  to  the  model 

where  the  discus  time  series  y^  and  u^  provide  data.  The  term  time  series  here  means 
a  sequence  of  data  ordered  in  time  [Ref.  10].  The  corresponding  identification  problem 
consists  in  determining  the  model  structure  and  parametric  estimation  of  the  polynomials 
involved.  In  many  cases,  such  parameters  can  be  identified  by  using  a  linear  regression 
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approach.  The  least-squares  solution  to  the  linear  regression  problems  exhibits  excellent 
pix^rties  in  cases  where  the  disturbances  at  different  sampling  times  are  uncorrelated. 
The  systematic  errors  in  cases  with  more  complicated  spectral  disturbance  characteristics 
constitute  a  significant  problem.  Conversely,  the  presence  of  correlated  disturbances  of 
composition  other  than  white  noise  renders  bias  reduction  necessary.  Efforts  to  solve 
such  problems  have  given  rise  to  several  extensions  of  linear  regression  models.  In 
particular.  Recursive  Least-Squares  (RLS)  Method  applied  to  the  estimation  of 
Autoregressive  Moving  Average  Extended  (ARMAX)  models  is  of  central  importance 
in  this  thesis. 

2.  ARMAX  Models  and  Difference  Equations 

The  ARMAX  models  constitute  an  important  class  of  difference  equations  on 

the  form 

where  z  '  is  a  time  delay  and  A,B,C  are  polynomials 

Aiz  ■*) = 1  +a,z  ' 

Biz  ■' 

C(z‘‘)=l+c,z*+...+c^‘ 

The  ARMAX  model,  in  equation  3.9,  is  completely  defmed  by  the  polynomials  A,  B, 
and  C.  So  we  can  group  all  unknown  coefficients  in  an  array 

which  completely  defines  the  model. 


(3.9) 


(3.10) 
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The  special  case  of  ARMAX  model  that  admits  a  reformulation  to  the  linear 
regression  model  is  the  controlled  autoregressive  extended  model  (ARX) 

A(z'^)y^=B(z'^)u^+w^  ,  (3.12) 

where  w,^  is  white  noise.  In  this  thesis,  we  deal  with  first  order  ARX  models. 

3.  Recursive  Estimation 

Real-time  application  of  identification  algorithms  is  interesting  for  various 
purposes  such  as  supervision,  tracking  of  time- varying  parameters  for  adaptive  control, 
filtering,  prediction,  signal  processing,  detection,  diagnosis  and  artificial  neural  networks 
[Ref.  9].  However,  most  identification  methods  based  on  a  set  of  measurements  are  not 
suitable  for  real-time  application.  It  is,  therefore,  desirable  to  make  a  suitable 
reformulation  of  the  algorithms  in  order  to  provide  efficient  procedures. 

There  are  several  attractive  features  of  recursive  estimation.  It  is  obviously  suitable 
for  real-time  tyjplications,  and  only  a  few  data  points  need  to  be  stored.  It  is  thus  a 
method  which  is  attractive  also  as  a  computational  method  of  off-line  algorithms.  In 
particular,  it  provides  a  method  for  identification  of  systems  with  time-varying 
parameters. 

There  are  also  certain  drawbacks  such  as  the  fact  that  the  model  structure  is 
determined  a  priori  and  the  fact  that  iterative  solutions  based  on  large  data  sets  may  be 
difficult  to  organize.  Thus,  it  is  of  some  interest  to  consider  the  desirable  modifications 
for  real-time  ai^lication  of  algorithms  originally  stated  as  off-line  methods.  The 
following  study  shows  one  such  simple  derivation. 
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Consider  an  ARX  model  of  the  form 


Mz  '^)y(t)=Biz  ‘WO  +«(0 

with  u(t),  y(t)  input, output  sequences,  and  e(t)  zero  mean  white  noise.  Also,  A  and  B  are 
polynomials  in  the  delay  operator  z*'  as 


A(z  ■‘) = 1 +a,z +a,^ ‘‘ 


(3.14) 


The  problem  is  to  estimate  the  parameters  a^  and  bj  from  the  data  {u}  and  {y}  in  a 
recursive  fashion.  In  order  to  do  so,  we  put  the  model,  in  equation  3.13,  in  Regression 
form  as 


=[  -y(^- 1),....  -y(t-n),4(f-  (3*15) 


In  the  light  of  equation  3.15  we  can  apply  Kalman  filtering  techniques  to  estimate  If 
we  assume,  ^  is  a  constant  vector,  we  can  write  the  following  state  space  equations 


y(0=4^(f)fi(0+e(0 


(3.16) 


Notice  that  equation  3.26  is  a  standard  state  space  equation,  where  e(t)  is  white  noise, 
and  we  can  easily  apply  Kalman  filtering  for  the  estimation  of  ^(t)  as 
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(3.17) 

x^+^^(OP(^)*a) 

with  =E(e(t)^].  It  seems  that  in  equation  3.17  we  need  to  know  ;  the  covariance 
of  die  error  e(t),  which  is  usually  not  known.  However,  in  practice  we  do  not  need  X^ . 
Just  define 


(3.18) 


The  rest  is  an  easy  exercise  to  show  that  the  recursive  least  squares  estimation  m^hod 
can  be  written  as 


l+^''(f)P(f)4(0  (3.19) 

Therefore,  P(t)  is  the  error  covariance  matrix,and  it  is  initialized  as  P(0)=pj[,  with  I 
identity  matrix,  p^  a  large  positive  value. 

4.  Simulation  Results 

So  far,  we  assumed  the  system  dynamics  introduced  in  the  following 
differential  equations  are  known. 
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v=-av+pu, 

@=-y6+oii2 


(3.20) 


At  this  point,  we  try  to  estimate  the  parameters  a,  /3,  7,  and  a  by  means  of  RLS, 
explained  above.  During  the  estimation  process,  we  try  to  fit  our  model  to  a  first  order 
ARX  model  by  using  the  sampled  values  of  V,  u,,  6,  and  Uj.  After  off-line  estimation  of 
discrete  parameters  (see  Fig.  3.10),  we  transform  discrete  time  parameters  to  continuous 
time  parameters  by  Laplace  Transforms  and  d)tain  our  a,  j3,  7,  and  a  parameters, 
which  will  be  used  in  further  simulations.  Ilie  source  code  used  to  do  the  simulations 
described  above  are  written  in  MATLAB  and  listed  in  ^^ndix-B. 

The  results  of  the  estimation  for  different  initial  conditions  are  shown  in  Figures 
3.11  through  3.18  ,  where  we  used  the  data  collected  for  seven  successive  360°  scans. 
In  all  the  figures,  the  sequence  of  the  estimated  reflected  surfaces  is  shown  for  each  of 
the  scans  superimposed  to  the  contours  of  the  pcrtential  function.  In  Figures  3.11  through 
3. 14,  we  assume  the  initial  position  and  orientation  of  the  sonar  head  to  be  known,  but 
the  velocity  and  direction  have  uncertainties.  In  Figures  3.15  through  3. 1 8,  we  repeat  the 
same  simulations  with  a  significant  error  in  the  initial  estimate  of  location.  In  Figures 
3. 13  and  3. 17,  we  introduce  one  slow  moving  object  on  poitside  of  our  vehicle,  and  two 
slow  moving  objects  in  Figures  3.14  and  3.18  on  both  sides  of  the  vehicle.  As  a  result 
of  these  simulations,  we  conclude  that  our  model  is  able  to  recover  by  using  potential 
function  qjproach  even  when  reasonable  numbers  of  obstacles,  which  are  not  on  the  mq), 
are  present  in  the  operation  area. 
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Figure  3.10  Off-Line  Estimated  Discrete  Time  Paiameteis 


Figure  3.11  Estimation  with  known  initial  location,  no  obstacles  are  present. 


44 


IV.  SUMMARY,  CONCLUSIONS,  AND  RECOMMENDATIONS 

A.  SUMMARY 

This  thesis  presents  a  study  of  model-based  estimator  for  small  autonomous 
underwater  vehicles.  The  approach  taken  in  the  design  and  testing  of  the  estimator 
included: 

•  The  development  of  linearized  models 

•  The  develc^ment  of  potential  function  for  the  environment 

•  The  estimation  of  parameters  by  means  of  an  ARX  model 

•  The  programming  in  MATLAB 

•  Simulation  studies  using  additive  white  Gaussian  noise 

B.  CONCLUSIONS 

In  this  study,  an  estimator  which  uses  knowledge  of  the  vehicle  dynamics  is 
developed.  In  particular,  position  estimation  is  obtained  by  combining  the  sonar  range 
information  with  inertial  measurements. 

The  following  conclusions  can  be  drawn  from  the  results  of  this  study: 

•  Position,  velocity  and  pitch  rate  estimation  is  possible. 

•  A  piecewise  constant  Extended  Kalman  gain  is  adequate  for  estimator. 

•  Estimation  of  states  by  Potential  function  technique  is  possible. 
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C.  RECOMMENDATIONS 

The  algorithm  explored  in  this  thesis  could  be  used  in  the  interim  for  the  NFS  AUV 
n  during  pool  missions.  It  is  therefore  recommended  that  the  following  be  accomplished 
to  facilitate  implementation  of  the  estimator: 

•  Convert  the  estimator’s  main  loop  and  other  MATLAB  functions  to  C 
and  compile  the  entire  program  for  use  in  the  NAPS  AUV  n  computer. 

•  Find  a  better  way  of  identifying  potential  functions  for  non-geometrical 
environments. 

•  Implement  neurol  netwoiks  to  identify  potential  functions. 

•  Try  to  eliminate  an  extra  controller  by  using  potential  functions  in 
obstacle  avoidance  problem. 


I 
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APPENDK  A 


A.  PROGRAM  STRUCTURE 

The  programs  are  PRDATA4.M,  RANGEH.M,  and  AUV5.M.  The  names  of  the 
source  code  files  for  different  versions  of  the  programs  are  appended  with  the  version 
number,  for  example,  PRDATA4.M. 

AUVS.M  is  the  main  source  code,  which  initializes  the  filter  parameters,  the 
simulated  vehicle  state,  the  Extended  Kalman  filter  state,  the  control  input,  and  the  #, 
A,,  and  H  matrices  of  the  linearized  model.  Hie  program  reads  the  data  Hie  as  a  standard 
MATLAB  matrix  instead  of  tunning  a  separate  vehicle  model  to  generate  simulated 
measurements. 

PRDATA4.M  is  the  source  code,  which  generates  the  simulated  measurements.  It 
also  saves  the  measurements  into  a  data  file  as  a  standard  MATLAB  matrix. 

RANGEH.M  is  the  source  code,  which  generates  the  estimated  nonlinear 
measurements  of  range.  It  also  calculates  the  gradient  of  range  with  respect  to  heading 
and  position  of  the  vehicle.  RANGEH.M  is  called  by  both  PRDATA4.M  and  AUVS.M. 
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B.  PROGRAM  LISTING  FOR  SIMULATIONS  IN  CHAPTER  H 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%  FUe  AUV5.M% 

%  This  little  program  is  designed  for  estimating  the  position  of  an  AUV  which 

%  moves  in  horizontal  plane.  We  assume  that  AUV  starts  from  a  known 

%  position  with  a  constant  heading  and  constant  speed.  In  this  model  we  also 

%  use  RPM  and  RUDDER  angle  as  the  inputs  to  the  system. 

% 

%  Calls  RANGEH.M 

% 

%  Modified  26  Jun  93 

% 

%  Ver.5 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

clear 

clg 

clc 

L1=6;L2=6;  %  pool  dimensions 

Ts«0.0225;  %entering  sampling  time 
load  datae25.dat 
clear  D 
D=datae2S; 

kmax=length(D)+l;  %discr^  time 
x0=[.8, 1.2, 0,1,0]’;  %initilization  of  states 

xk  1  k =zeros(S  ,kmax) ; 
xklk(;,l)=x0; 

P0=diag([10  10  0  10  0]); 
pklk=P0; 

R=0.1; 

alfa=l; 

beta=0.001; 

sigma=l; 

gama=l; 

for  k=l:kmax-l 

headh=xklk(4,k);  %heading  of  vehicle 

theta=D(k,l); 

riio=D(k,2); 

RPM(k)=D(k,6); 


RUDDER(k)=D(k,7); 


a =cos(headh''t)i/ 1 80) ; 

b = sin(headh*pi/ 1 80) ; 

A=[  OOaOO 
OObOO 
0  0  -alfa  0  0 
0000  1 
0  0  0  0  -gama]; 

B=[0  0;0  0;beta  0;0  0;0  sigma]; 
[phi, dell]  =c2d(A,B,Ts); 


shdg = theta + headh ; 
if  shdg  >  180 
shdg = -360 + shdg ; 
end 

[rhoh.ho]=rangeh(xklk(l,k).xklk(2,k),shdg,Ll,L2);  %sonar  range 

drdx=ho(l,2); 

drdy=ho(l,3); 

h=[drdx  drdy  0  0  0]; 

KG=pklk*h7(h*pklk*h’  -l-R); 

pklkl  =(eye(5)-KG*h)*pklk; 

pklk=phi‘^klkl*phi’; 

xklkl  (:  ,k) =xklk(:  ,k)  +KG*(iho-fhoh); 

xklk(:  ,k+ 1)  =phi*xklkl (:  ,k) +dell  *[RPM(k)  RUDDER(k)] ’ ; 

xp(k)=xklkl(l,k)-»-rhoh*cos(shdg*pi/180); 

yp(k)=xklkl(2,k)-»-i1ioh*sin(shdg*pi/180); 

end 

axis([-l  Ll-Hl  -1  L2-H]) 

plot(xklkl(l,:),xklkl(2,:),D(;,4),D(:,5),’g\xP.yP.’*’).i”d 

xlabel(’x’).ylabel(’y’),titIe(’POOL’) 

!del  tezg25.met 
meta  tezg25 
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%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

% 

%  FUe  PRDATA4.M 
% 

%  This  little  program  is  desigiud  for  creating  data.  We  assume  that  AUV  sbtts 

%  from  a  known  position  with  a  constant  heading  and  constant  speed.We  also 

%  use  the  RPM  and  RUDDER  angle  as  the  input.  In  this  program  we  also  added 

%  small  noises  to  the  states. 

% 

%  CaUs  RANGEH.M 
% 

%  Modified  26  Jun  93 
% 

%  Ver.4 
% 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


clear 

clg 

clc 

LI  =6;L2=6;  %pool  dimmisions 

Ts»0.022S;  %entering  sampling  time 

'n’=9;  %time  required  for  a  360  degrees  return 

kmax=9*Tf/Ts;  %discrete  time 

x=zetos(S,kmax); 

x(:,l)=[l  1  0  315  0]’; 

RPM= 150*ones(l  ,kmax); 

RUDDER =zeros(  1  ,kmax) ; 

alfa=l; 

beta=0.001; 

sigma=l; 

gama=l; 

for  k=l:kmax-l 
tetha=x(4,k); 
a = cos(tetha*pi/ 1 80) ; 
b =sin(t^ha*piy  1 80); 
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A=[  OOaOO 
OObOO 
0  0  -alfa  0  0 
0000  1 
0  0  0  0  -gama]; 

B=[0  0;0  0;beta  0;0  0;0  sigma]; 
E=eye(5); 

[phi, dell]  =c2d(A,B,Ts); 
[phi,del2]=c2d(A,E,Ts); 


RUDDBR(k)=5; 


rand(’nomial’) 
ex=0.01*rand; 
ey=0.01*iand; 
ev  =0.01  •land; 
et=0.01*rand; 
etd=0.01*rand; 

x(:,k+l)=phi»x(:,k)+dell*[RPM(k)  RUDDER(k)]’+del2*[ex  ey  ev  et  etd]’; 
h^d(k)=x(4,k); 

shdg(k)=rem(head(k)+0.9*k,360);  %sonar  heading 

shdgl  (k) =rem(0.9*k,360); 
ifshdg(k)  >  180 
shdg(k) =-360+shdg(k); 
end 

if  shdgl(k)  >  180 

shdgl  (k) = -360+ shdgl  (k) +0.0 1  •raiKi; 
end 

[r,h]=iangeh(x(l,k),x(2,k),shdg(k),Ll,L2);  %sonar  range 
dist(k) =r+0.01  *rand; 

temp(k, :) =[shdgl(k)  dist(k)  x(3,k)  x(l  ,k)  x(2,k)  RPM(k)  RUDDER(k)  x(4,k)  x(5,k)]; 
end 

!del  datae33.dat 

save  datae33.dat  temp  /ascii  %saving  data  file  in  ASCII  code 

axis(’ square’) 
axis([0  LI  0  L2]) 
plot(x(l,:),x(2,:)),grid 
title(’POOL’) 
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%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

% 

%  File  l^GEH.M 
% 

%  [r,h] =range(x, y, theta, LI  ,L2) 

%  computes  the  sonar  range  (r)  and  its  gradient  (h) 

%  at  position  (x,y)  with  heading  theta 

%  in  the  LI  by  L2  pool. 

%  h=[dr/dtheta,  dr/dx,  dr/dy] 

% 

%  Modified  26  Jun  93 
% 

%  Called  by  AUV5.M  and  PRDATA4.M 
% 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

function  [r,h] =rangeh(x,y, theta, LI  ,L2) 
theta=theta*pi/ 1 80; 
thl  =atan2(-y,-x); 
th2  =atan2(L2-y,-x); 
th3  =atan2(L2-y,Ll-x); 
th4=atan2(-y,Ll -x); 


while  (theta  >thl+2*pi), 
theta— theta-2*pi; 
end 

while  (theta  <  =th4), 

theta=theta+2*pi; 

end 

c = cos(theta) ;  s = sin(theta) ; 

if  (theta  >  =th2)&(theta<thl+2*pi), 
r=-x/c; 

h=[-x*s/c^2,-l/c,0]; 

end 

if  (theta>  =th3)&(theta<th2), 
r=(L2-y)/s; 

h=[(y-L2)*c/s^2,0,-l/s]; 

end 


if  (theta  >  =th4)&(theta<th3). 
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end 


r=(Ll-x)/c; 

b=((Ll-x)*s/c*2,-l/c,0]; 


if  (tb^>  =thl+2*pi)&(theta<th4+2*pi), 
r=-y/s; 

h=[-y*c/s^2,0.-l/s]; 
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APPENDKB 


A.  PROGRAM  STRUCTURE 

The  programs  are  SIMUL5.M,  SCAN5.M,  VP5.M,  SIGMOID.M,  DSIG.M, 
SHOWS.M,  SIMUL8.M,  and  SCAN8.M.  The  names  of  the  source  code  files  for 
different  versions  of  the  programs  are  appended  with  the  version  number,  for  example, 
SIMUL8.M. 

SIMULS.M  is  the  main  source  code,  which  initializes  the  filter  parameters,  the 
simulated  vehicle  state,  the  Extended  Kalnum  filter  state,  the  control  input,  and  the 
A,,  and  H  matrices  of  the  linearized  model.  The  program  reads  the  data  file  as  a  standard 
MATLAB  matrix  instead  of  running  a  separate  vehicle  model  to  generate  simulated 
measurements.  In  this  source  code  the  dynamic  parameters  of  the  vehicle  are  assumed 
to  be  known. 

SCAN5.M  is  designed  for  estimating  the  states  of  the  vehicle  using  the  potential 
function  s^roach.  It  scans  the  estimated  borders  of  the  map.  VPS.M,  SIGMOID.M, 
DSIG.M,  and  SHOWS.M  are  designed  to  calculate  the  value  of  the  potential  function, 
the  value  of  sigmoid  function,  the  derivatives  of  sigmoid  function  and  to  show  the 
graphics  respectively. 

SIMULS.M  and  SCAN8.M  are  basically  same  with  the  SIMULS.M  and 
SCANS.M.  The  only  difference  introduced  in  these  versions  is  the  estimation  of  the 
dynamic  parameters  of  the  vehicle  by  using  an  ARX  model  and  RLS  method. 


B.  PROGRAM  LISTING  FOR  SIMULATIONS  IN  CHAPTER  HI 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

% 

%  File  SIMUL5.M 
% 

%  This  source  code  is  designed  for  estimating  the  position  of  an  AUV  which 

%  moves  in  horizontal  plane.  We  assume  that  AUV  starts  with  a  constant 

%  heading  and  ^)eed.  In  this  mottel  we  use  RPM  and  RUDDER  angle  as  the 

%  inputs  to  the  AUV  system. 

% 

%  Calls  SCAN5.M,  SHOW5.M 

% 

%  Modified  9  Jul  93 

% 

%  Ver.5 

Otltt.OtCtOtOt^tt.CtOtOLOtOt.Ot.OLCtQtQtOLCLCtOLatOtQtOLatatQtCtOtOtOtOtOLQt.OtatOtCi.Ot 


clear; 

clg; 

hold  off; 
clc 

subplot(221) 

load  c:\mat\tra\data\datae32.dat  %  data  of  pool 

data=datae32; 

n=length(data); 


nl  =  l;  n2  =400; 
lambda =50; 
zhm=2eros(5,l); 

P=diag([10,10,10,10,10]);  zh0=[3,l, 0,0,0]’;  %  initialize  estimate 

i=0; 

while  n2<n, 

[zh,P,zs] =scanS(zhO,P,data(nl  :n2,:),lambda); 

xm=zs(l,:);  ym=zs(2,:); 

zhm=[zhm,  zh]; 

i=i+l; 

shows 

if  i==4 

meta  tezg37 
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end 

hold  off 

nl=n2;  n2=niin([nl+400,n]); 
lanibda=0.5*lainbda; 
nz=:length(zh);  zh0=2h(:,nz); 
end 

hold  off 
show5 
hold  on 

pl(M(zhni(l,;),  zhm(2,:),’*g’),  title(’  E^imated  Trajectory’) 

meta 

end 
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%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

% 

%  FUe  SCAN5.M 
% 

%  This  source  code  is  designed  for  estimating  the  position  for  static  point  using 

%  potential  fiiction.  It  scans  the  estimated  borders  of  the  map 

% 

%  Calls  VP5.M 

% 

%  Modified  8  Jul  93 

% 

%  Ver.5 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


function  [zh,P,zs]=scan5(zhO,P, data, lambda) 

n=length(data); 

zh(:,l)=zhO; 

R=l* 

alfa=  1 1 .2957;beta= .01 1 1  ;sigma= 1 .0023;gama=  1 .0023;Ts =0.0225; 
for  t=l:n-l 

iho=data(t,2);  theta=data(t,l);  headh=zh(4,t); 

RPM(t)=data(t,6);  RUDDER(t)=data(t,7); 
a=cos(headh*pi/180);  b=sin(headh*pi/180); 

A=[0  0  a  0  0 
OObOO 
0  0  -alfa  0  0 
0000  1 
0  0  0  0  -gama]; 

B=[0  0;0  0;beta  0;0  0;0  sigma]; 

[Phi,del]=c2d(A,B,Ts); 

dxO = rho*cos((theta+ headh)  *pi/ 1 80) ; 

dyO = rho*sin((theta + headh)  *pi/ 1 80) ; 

x0=zh(l,t)+dx0; 

y0=zh(2,t)+dy0; 

zs(:,t)=[x0;y0]; 

[v,dvx,dvy]  =VP5(x0,y0, lambda); 
h =[dvx,dvy,0,(-dvx*dy0+dvy  *dx0)*pi/ 1 80,0]’ ; 
s=h’*P*h+l;  K=Phi*P*h/s; 
e=0-v; 

zh(:  ,t+ 1)  =Phi*zh(;  ,t) +del*[RPM(t)  RUDDER(t)]’  +K*e; 

P=Phi*p*Phi’-K*s’TC’; 

end 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

% 

%  FUe  VP5.M,  SIGMOID.M,  DSIG.M,  SHOW5.M 
% 

%  These  source  codes  are  designed  for  calculating  the  potential  function,  the 

%  value  of  sigmoid,  its  derivative  at  a  given  point  and  to  show  graphics. 

% 

%  Modified  9  Jul  93 
% 

%  Ver.5 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


function  [v,dvx,dvy]=VP5(x,y, lambda); 

v0=(x.*(x-6))*(y.*(y-6))’; 

z=(vO)/(lambda);  v=sigmoid(z); 

dvx  =dsig(z)*((x-6)’^(y .  *(y-6))’  +x*{y.  *(y-6))’)/lambda; 

dvy =dsig(z)*((x.  *(x-6))*(y-6)’ + (x.  *(x-6))*y’)/lambda; 

end  %  VPS 

function  y=sigmoid(x) 
x=min(x,100);  x=max(x,-100); 
y=  (l-exp(-x))./(l+exp(-x)); 
end  %  SIGMOID 

function  d=dsig(x) 

%  derivative  of  sigmoid 
x=min(x,100); 
x=max(x,-100); 
temp=exp(-x); 

d=temp  ./  (l+2*temp  +  temp  *  temp); 
end  %DSIG 

%SHOW5.M 

x=-l:.l;7; 

y=-l:,l:7; 

[v, dvx, dvy]  =VP5(x’ ,y’ .lambda); 

contour(v,x,y) 

hold  on 

for  t = 1  :length(xm) 
plot(xm(t),  ym(t),  ’og’) 
end 

end  %show 
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%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

% 

%  FUe  SIMUL8.M 
% 

%  This  source  code  is  designed  for  estimating  the  position  of  an  AUV  which 

%  moves  in  horizantal  plane.  We  assume  that  AUV  starts  with  a  constant 

%  heading  and  ^)eed.  In  this  model  we  use  RPM  and  RUDDER  angle  as  the 

%  inputs  to  the  AUV  system.  Also  we  try  to  fit  the  heading  rate  and  speed  data 

%  to  an  ARX  model  by  using  RLS. 

% 

%  Calls  SCAN8.M,  SHOW5.M 

% 

%  Modified  10  Aug  93 

% 

%  Ver.8 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


!del  tezg46.met 

.'del  paia8.met 

clear;  clg;  hold  off;clc 

subplot(221) 

load  datae2S.dat 

data=datae25; 

n-length(data); 

Ts=0.0225; 

%ESTIMATION  OF  SYSTEM  PARAMETERS  BY  USING  RLS  AND 
%FIRST  ORDER  ARX  MODEL 

^ESTIMATION  FOR  SPEED  PARAMETERS 
vl  =data(:,3);  %MEASURED  SPEED  DATA 

ul  =data(: ,6);  %RPM  INPUT 

thl  =zeros(2,n); 

Pl  =  10*eye(2); 
for  kl  =2:n-l 

phitl(kl,;)=[-vl(kl-l)  ul(kl-l)]; 
deni  (kl) = 1 +phitl  Gtl ,  "^hitl  (kl , ; 

Kl(:,kl)=Pl’^hitl(kl,:)7denl(kl); 
Pl=Pl-Pl*phitl(kl,:)’*phitl(kl,;)*Pl/denl(kl); 
thl(:,kl  +  l)=thl(;,kl)+Kl(:,kl)*(vl(kl)-phitl(kl,:)nhl(:,kl)); 
end 
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^ESTIMATION  FOR  YAW  RATE  PARAMETERS 
v2=clata(:,9);  %MEASURED  HEADING  DATA 

u2 =data(:  ,7);  %RUDDER  ANGLE  INPUT 

th2=zeros(2,n); 

P2  =  10*eye(2); 
for  k2=2:n-l 

phit2(k2,;)=[-v2(k2-l)  u2(k2-l)]; 
den2(k2)= 1  +phit2(k2,:)*P2'^hit2(k2,;)’; 

K2(:  ,k2)  =P2*phit2(k2,;)’/deo2(k2); 

P2 =P2-P2’^hit2(k2,  :)’*phit2(k2,:)*P2/den2(k2); 
th2(:  ,k2 + 1)  =th2(;  ,k2) +K2(:  ,k2)*(v2(k2)-phit2(k2,  :)nh2(:  ,k2)); 
end 

%TRANSFORMING  PARAMETERS  FROM  DISCRETE  TO  CONTINUOUS  TIME 
phil=[-thl(l,n)  0;0  -th2(l,n)]; 
dell=[thl(2,n)  0;0  th2(2,n)]; 

[A1  ,B1]  =d2c(phil  ,dell  ,Ts); 
prms=[-Al(l,l);Bl(l,l);-Al(2.2);Bl(2,2)]; 

%PLOTING  THE  DISCRETE  PARAMETERS 
tl=0:n-l; 
t  =0.0225 *11; 

plot(t,-thl(l ,:));titIe(’ALFA  ESTIMATE’);xlabel(’Time’);grid 

plot(t,thl(2,:));title(’BETA  ESTIMATE’);xlabel(’Tinie’);gTid 

plot(t,-th2(l ,  :));tiUe(’GAMA  ESTIMATE’);xlabel(’Time’);grid 

plot(t,th2(2,:));titie(’SIGMAESTIMATE’);xlabel(’Time’);grid 

pause 

meta  para9 

clg 

^SIMULATION  DUE  TO  PARAMETERS  ESTIMATED  ABOVE 

axis(’normal’) 
subplot(221) 
nl  =  l;  n2  =400; 

Iambda=200; 

zhm=zeros(5,l); 

P=diag([100,100,100,100,100]);  zh0=[2,3,l,10,10]’;  %  initialize  estimate 

i=0; 

while  n2<n, 
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[zh,P,zs] =scan8(zhO,P,data(iil  :n2, 0.lambda.prms); 

xin=zs(l,:);  yin=zs(2,;); 

zhin=[zhin,  zh]; 

i=i+l; 

shows 

if  i==4 

meta  tezg46 

end 

hold  off 

nl=n2;  n2=min([nl+400,n]); 
lambda =0.S*lambda; 
nz=length(zh);  zhO=zh(:,nz); 
end 

hold  off 
shows 
hold  on 

plot(zlun(l,:),  zhm(2,:),’*g’),  title(’  Estimated  Trajectory’) 
meta 

end  %  simulation 

!del  den.dat 

save  den.dat  ziim  /ascii 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

% 

%  File  SCAN8.M 
% 

%  This  source  code  is  designed  for  estimating  the  position  for  static  point  using 

%  potential  fuction.  It  scans  the  estimated  borders  of  the  map 

% 

%  Calls  VP5.M 

% 

%  Modified  8  Aug  93 

% 

%  Ver.8 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


function  [zh,P,zs]  =scan8(zhO,P, data, lambda, prms) 

n=length(data); 

zh(:,l)=zhO; 

R=l; 

alfa =pnns(  1  );beta =pims(2);  sigma =prms(3);gama =prms(4)  ;Ts =0.0225 ; 
for  t=l:n-l 

rfao=data(t,2);  th^=data(t,l);  headh=zh(4,t); 

RPM(t)=data(t,6);  RUDDER(t)=data(t,7); 
a=cos(headh*pi/i80);  b=sin(headh*pi/180); 

A=[0  0  a  0  0 
OObOO 
0  0  -alfa  0  0 
0000  1 
0  0  0  0  -gama]; 

B=[0  0;0  0;beta  0;0  0;0  sigma]; 

[Phi,del]=c2d(A,B,Ts); 

dx0=rho*cos((theta-l-headh)*pi/180); 

dyO = iho*sin((theta + headh)’^i/ 1 80) ; 

x0=zh(l,t)+dx0; 

yO=zh(2,t)+dyO; 

zs(;,t)=[x0;y0]; 

[v,dvx,dvy]  =  VP5(x0,y0,lambtj  4; 
h = [dvx,dvy  ,0,(-dvx*dy0-l-dvy*dx0)'^i/l  80,0]’ ; 
s=h’*P*h-!-l;  K=Phi*P*h/s; 
e=0-v; 

zh(:,t-l- l)=Phi*zh(:,t)-»-del*[RPM(t)  RUDDER(t)]’  +K*e; 

P=Phi*P*Phi’-K*s*K’; 

end 
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